#ifndef URSCRIPT_HPP
#define URSCRIPT_HPP


#include <string>
#include <sstream>
#include <fstream>
#include <math.h>

class URScript
{
public:
    template<typename T>
    static std::string SetTCPPoseScript(T x, T y, T z, T rx, T ry, T rz, T a=0.02, T v=0.1, char f='j', T r=0)
    {
        std::ostringstream oss;
        oss<<"move" << f << "(p["
          <<x<<","
         <<y<<","
        <<z<<","
        <<rx<<","
        <<ry<<","
        <<rz<<"], a="
        <<a<<", v="
        <<v<<", r="
        <<r<<")\n";
        return oss.str();
    }


    template<typename T>
    static std::string SetTCPArcScript(T x1, T y1, T z1, T rx1, T ry1, T rz1,
                                       T x2, T y2, T z2, T rx2, T ry2, T rz2,
                                       T r=0.01, T a=0.02, T v=0.1)
    {
        std::ostringstream oss;
        oss<<"movec(p["
          <<x1<<","
         <<y1<<","
        <<z1<<","
        <<rx1<<","
        <<ry1<<","
        <<rz1<<"], p["
        <<x2<<","
        <<y2<<","
        <<z2<<","
        <<rx2<<","
        <<ry2<<","
        <<rz2<<"], a="
        <<a<<", v="
        <<v<<", r="
        <<r<<")\n";
        return oss.str();
    }


    static double RGZOffset(double halfOpenmm=0, double Rmm=55, double halfOpenSamplemm=89.8/2, double zOffsetSamplemm=28.23)
    {
        double delta=sqrt(4*Rmm*Rmm/(halfOpenSamplemm*halfOpenSamplemm+zOffsetSamplemm*zOffsetSamplemm)-1);
        double centerXmm=(halfOpenSamplemm-zOffsetSamplemm*delta)/2;
        double centerYmm=(zOffsetSamplemm+halfOpenSamplemm*delta)/2;

//        double a=centerXmm*centerXmm+centerYmm*centerYmm-Rmm*Rmm;

//        double b=(centerXmm-halfOpenSamplemm)*(centerXmm-halfOpenSamplemm)+(centerYmm-zOffsetSamplemm)*(centerYmm-zOffsetSamplemm)-Rmm*Rmm;

        double zOffsetmm=centerYmm-sqrt(Rmm*Rmm-(halfOpenmm-centerXmm)*(halfOpenmm-centerXmm));

        return zOffsetmm;
    }

    static double RG2ZOffset(double halfOpenmm)
    {
        return RGZOffset(halfOpenmm, 55, 89.8/2, 28.23);
    }


    static double RG6ZOffset(double halfOpenmm)
    {
        return RGZOffset(halfOpenmm, 80, 103.4/2, 946.53-927.43);
    }


    template<typename T>
    static std::string RG2Script(T openmm)
    {
        int bit=8916+4*openmm;
        std::ostringstream oss;
        oss<<"  set_tcp(p[-0,0,0.18824,-0,0,0.7854])\n"
          <<"  	timeout = 0\n"
         <<"  	while get_digital_in(9) == False:\n"
        <<"  	  if timeout > 400:\n"
        <<"  	    break\n"
        <<"  	  end\n"
        <<"  	  timeout = timeout+1\n"
        <<"  	  sync()\n"
        <<"  	end\n"
        <<"  	def bit(input):\n"
        <<"  	  msb=65536\n"
        <<"  	  local i=0\n"
        <<"  	  local output=0\n"
        <<"  	  while i<17:\n"
        <<"  	    set_digital_out(8,True)\n"
        <<"  	    if input>=msb:\n"
        <<"  	      input=input-msb\n"
        <<"  	      set_digital_out(9,False)\n"
        <<"  	    else:\n"
        <<"  	      set_digital_out(9,True)\n"
        <<"  	    end\n"
        <<"  	    if get_digital_in(8):\n"
        <<"  	      out=1\n"
        <<"  	    end\n"
        <<"  	    sync()\n"
        <<"  	    set_digital_out(8,False)\n"
        <<"  	    sync()\n"
        <<"  	    input=input*2\n"
        <<"  	    output=output*2\n"
        <<"  	    i=i+1\n"
        <<"  	  end\n"
        <<"  	  return output\n"
        <<"  	end\n"
        <<"  	bit("<<bit<<")\n"
        <<"  	timeout = 0\n"
        <<"  	while get_digital_in(9) == True:\n"
        <<"  	  timeout = timeout+1\n"
        <<"  	  sync()\n"
        <<"  	  if timeout > 20:\n"
        <<"  	    break\n"
        <<"  	  end\n"
        <<"  	end\n"
        <<"  	timeout = 0\n"
        <<"  	while get_digital_in(9) == False:\n"
        <<"  	  timeout = timeout+1\n"
        <<"  	  sync()\n"
        <<"  	  if timeout > 400:\n"
        <<"  	    break\n"
        <<"  	  end\n"
        <<"  	end\n"
        <<"  	if get_digital_in(8) == False:\n"
        <<"  		set_payload(0.738,[0,0,0.06551])\n"
        <<"  	else:\n"
        <<"  		set_payload(1.238,[-0,0,0.11508])\n"
        <<"  	end\n"
        <<"  	global lost_grip=False\n"
        <<"  	global grip_detected=False\n"
        <<"  	sync()\n"
        <<"  	sync()\n"
        <<"  	sync()\n"
        <<"  	if True == get_digital_in(8):\n"
        <<"  		grip_detected=True\n"
        <<"  	end\n"
        <<"  	qscale = (get_analog_in(2)-0.026)/2.976\n"
        <<"  	qangle = qscale*1.57079633-0.087266462\n"
        <<"  	qwidth = 5+110*sin(qangle)\n"
        <<"  	global measure_width = (floor(qwidth*10))/10-9.2\n"
        <<"  set_tcp(p[0,0,0,0,0,0])\n";
        return oss.str();
    }


    template<typename T>
    static std::string RG6Script(T target_width=160, T target_force=25, T payload=0.0,
                                 std::string set_payload="True", std::string depth_compensation="False", std::string slave="False")
    {

        std::ostringstream oss;

        oss<<"  set_standard_analog_input_domain(0, 1)\n"
        <<"  set_standard_analog_input_domain(1, 1)\n"
        <<"  set_tool_analog_input_domain(0, 1)\n"
        <<"  set_tool_analog_input_domain(1, 1)\n"
        <<"  set_analog_outputdomain(0, 0)\n"
        <<"  set_analog_outputdomain(1, 0)\n"
        <<"  set_tool_voltage(24)\n"
        <<"  set_input_actions_to_default()\n"
        <<"  set_tcp(p[0.0,0.0,0.0,0.0,0.0,0.0])\n"
        <<"  set_payload(0.16)\n"
        <<"  set_gravity([0.0, -1.2026031567627009E-15, -9.82])\n"
        <<"  rtde_set_watchdog(\"speed_slider_mask\", 10.0, \"ignore\")\n"
        <<"  # begin: URCap Installation Node\n"
        <<"  #   Source: Robotiq_Force_Torque_Sensor, 1.1.1, Robotiq Inc.\n"
        <<"  #   Type: FT Sensor\n"
        <<"  ###############################################################\n"
        <<"  # Script file used to communicate with Robotiq's ft sensor\n"
        <<"  # Version: 0.0.1\n"
        <<"  ###############################################################\n"
        <<"  \n"
        <<"  def rq_move_relative(P_from, P_to, Pi):\n"
        <<"    return pose_trans(P_to, pose_trans(pose_inv(P_from), Pi))\n"
        <<"  end\n"
        <<"  \n"
        <<"  def rq_elementWiseSub(list1, list2):\n"
        <<"    global rq_return_list=list1\n"
        <<"    i=0\n"
        <<"    while i<length(list1):\n"
        <<"      rq_return_list[i]=list1[i] - list2[i]\n"
        <<"      i=i+1\n"
        <<"    end\n"
        <<"    return rq_return_list\n"
        <<"  end\n"
        <<"  \n"
        <<"  if (not socket_open(\"127.0.0.1\",63351,\"stream\")):\n"
        <<"   popup(\"Can't connect to the sensor driver\", \"Robotiq\'s FT Sensor\", error=True)\n"
        <<"  end\n"
        <<"  # end: URCap Installation Node\n"
        <<"  # begin: URCap Installation Node\n"
        <<"  #   Source: Robotiq_Wrist_Camera, 1.2.1.R01, Robotiq Inc.\n"
        <<"  #   Type: Camera\n"
        <<"  \n"
        <<"  ###########################################\n"
        <<"  #######Vision urcap preamble start########\n"
        <<"  \n"
        <<"  logging_service = rpc_factory(\"xmlrpc\",\"http://127.0.0.1:4747\")\n"
        <<"  # Converts a pose relative to the flange in the base frame.\n"
        <<"  def get_T_in_base_from_flange(T_x_in_flange):\n"
        <<"  \n"
        <<"    T_flange_in_base = get_actual_tool_flange_pose()\n"
        <<"  \n"
        <<"    T_x_in_base = pose_trans(T_flange_in_base, T_x_in_flange)\n"
        <<"  \n"
        <<"    return T_x_in_base\n"
        <<"  end\n"
        <<"  \n"
        <<"  # Search pose cartesian (camera pose)\n"
        <<"  T_camera_in_flange = p[0.0, 0.05, 0.05, -0.5, 0.0, 0.0]\n"
        <<"  snapshot_position_offset = p[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]\n"
        <<"  ignore_snapshot_position = False\n"
        <<"  \n"
        <<"  # Open connection with vision service\n"
        <<"  xmlrpc_server=rpc_factory(\"xmlrpc\",\"http://127.0.0.1:4242\")\n"
        <<"  \n"
        <<"  #######Vision urcap preamble end##########\n"
        <<"  ###########################################\n"
        <<"  \n"
        <<"  # end: URCap Installation Node\n"
        <<"  # begin: URCap Installation Node\n"
        <<"  #   Source: RG - On Robot, 1.2.1, On Robot ApS\n"
        <<"  #   Type: RG6 Configuration\n"
        <<"  global measure_width=0\n"
        <<"  global grip_detected=False\n"
        <<"  global lost_grip=False\n"
        <<"  global zsysx=0\n"
        <<"  global zsysy=0\n"
        <<"  global zsysz=0.10205\n"
        <<"  global zsysm=1.085\n"
        <<"  global zmasx=-0\n"
        <<"  global zmasy=-0\n"
        <<"  global zmasz=0.23434\n"
        <<"  global zmasm=0\n"
        <<"  global zmasm=0\n"
        <<"  global zslax=0\n"
        <<"  global zslay=0\n"
        <<"  global zslaz=0\n"
        <<"  global zslam=0\n"
        <<"  global zslam=0\n"
        <<"  thread lost_grip_thread():\n"
        <<"  while True:\n"
        <<"  set_tool_voltage(24)\n"
        <<"  	if True ==get_digital_in(9):\n"
        <<"  		sync()\n"
        <<"  		sync()\n"
        <<"  		sync()\n"
        <<"  		if True == grip_detected:\n"
        <<"  			if False == get_digital_in(8):\n"
        <<"  				grip_detected=False\n"
        <<"  				lost_grip=True\n"
        <<"  			end\n"
        <<"  		end\n"
        <<"  	set_tool_analog_input_domain(0, 1)\n"
        <<"  	set_tool_analog_input_domain(1, 1)\n"
        <<"  	zscale = (get_analog_in(2)-0.026)/2.976\n"
        <<"  	zangle = zscale*1.57079633-0.0942477796\n"
        <<"  	zwidth = 8.4+160*sin(zangle)\n"
        <<"  	global measure_width = (floor(zwidth*10))/10-9.2\n"
        <<"  	end\n"
        <<"  	sync()\n"
        <<"  end\n"
        <<"  end\n"
        <<"  lg_thr = run lost_grip_thread()\n"
        <<"  def RG6(target_width=160, target_force=120, payload=0.0, set_payload=False, depth_compensation=False, slave=False):\n"
        <<"  	grip_detected=False\n"
        <<"  	if slave:\n"
        <<"  		slave_grip_detected=False\n"
        <<"  	else:\n"
        <<"  		master_grip_detected=False\n"
        <<"  	end\n"
        <<"  	timeout = 0\n"
        <<"  	while get_digital_in(9) == False:\n"
        <<"  	  if timeout > 400:\n"
        <<"  	    break\n"
        <<"  	  end\n"
        <<"  	  timeout = timeout+1\n"
        <<"  	  sync()\n"
        <<"  	end\n"
        <<"  	def bit(input):\n"
        <<"  	  msb=65536\n"
        <<"  	  local i=0\n"
        <<"  	  local output=0\n"
        <<"  	  while i<17:\n"
        <<"  	    set_digital_out(8,True)\n"
        <<"  	    if input>=msb:\n"
        <<"  	      input=input-msb\n"
        <<"  	      set_digital_out(9,False)\n"
        <<"  	    else:\n"
        <<"  	      set_digital_out(9,True)\n"
        <<"  	    end\n"
        <<"  	    if get_digital_in(8):\n"
        <<"  	      out=1\n"
        <<"  	    end\n"
        <<"  	    sync()\n"
        <<"  	    set_digital_out(8,False)\n"
        <<"  	    sync()\n"
        <<"  	    input=input*2\n"
        <<"  	    output=output*2\n"
        <<"  	    i=i+1\n"
        <<"  	  end\n"
        <<"  	  return output\n"
        <<"  	end\n"
        <<"  	target_width=target_width+9.2\n"
        <<"  	if target_force>120:\n"
        <<"  	target_force=120\n"
        <<"  	end\n"
        <<"  	if target_force<25:\n"
        <<"  	target_force=25\n"
        <<"  	end\n"
        <<"  	if target_width>160:\n"
        <<"  	target_width=160\n"
        <<"  	end\n"
        <<"  	if target_width<0:\n"
        <<"  	target_width=0\n"
        <<"  	end\n"
        <<"  	rg_data=floor(target_width)*4\n"
        <<"  	rg_data=rg_data+floor(target_force/5)*4*161\n"
        <<"  	if slave:\n"
        <<"  	rg_data=rg_data+16384\n"
        <<"  	end\n"
        <<"  	bit(rg_data)\n"
        <<"  	if depth_compensation:\n"
        <<"  	finger_length = 80.0/1000\n"
        <<"  	finger_heigth_disp = 6.3/1000\n"
        <<"  	center_displacement = 10.5/1000\n"
        <<"  \n"
        <<"  	start_pose = get_forward_kin()\n"
        <<"  	set_analog_inputrange(2, 1)\n"
        <<"  	zscale = (get_analog_in(2)-0.026)/2.976\n"
        <<"  	zangle = zscale*1.57079633-0.0942477796\n"
        <<"  	zwidth = 8.4+160*sin(zangle)\n"
        <<"  \n"
        <<"  	start_depth = cos(zangle)*finger_length\n"
        <<"  \n"
        <<"  	sync()\n"
        <<"  	sync()\n"
        <<"  	timeout = 0\n"
        <<"  	while get_digital_in(9) == True:\n"
        <<"  	  timeout=timeout+1\n"
        <<"  	  sync()\n"
        <<"  	  if timeout > 20:\n"
        <<"  	    break\n"
        <<"  	  end\n"
        <<"  	end\n"
        <<"  	timeout = 0\n"
        <<"  	while get_digital_in(9) == False:\n"
        <<"  	  zscale = (get_analog_in(2)-0.026)/2.976\n"
        <<"  	  zangle = zscale*1.57079633-0.0942477796\n"
        <<"  	  zwidth = 8.4+160*sin(zangle)\n"
        <<"  	  measure_depth = cos(zangle)*finger_length\n"
        <<"  	  compensation_depth = (measure_depth - start_depth)\n"
        <<"  	  target_pose = pose_trans(start_pose,p[0,0,-compensation_depth,0,0,0])\n"
        <<"  	  if timeout > 400:\n"
        <<"  	    break\n"
        <<"  	  end\n"
        <<"  	  timeout=timeout+1\n"
        <<"  	#  servoj(get_inverse_kin(target_pose), t=0.008, lookahead_time=0.033, gain=1500)\n"
        <<"  	#  textmsg(point_dist(target_pose, get_forward_kin()))\n"
        <<"  	#end\n"
        <<"  	#textmsg(\"end gripper move!!!!!\")\n"
        <<"  	#nspeedthr = 0.001\n"
        <<"  	#nspeed = norm(get_actual_tcp_speed())\n"
        <<"  	#while nspeed > nspeedthr:\n"
        <<"  	#  servoj(get_inverse_kin(target_pose), t=0.008, lookahead_time=0.033, gain=1500)\n"
        <<"  	#  nspeed = norm(get_actual_tcp_speed())\n"
        <<"  	#  textmsg(point_dist(target_pose, get_forward_kin()))\n"
        <<"  	#end\n"
        <<"  	servoj(get_inverse_kin(target_pose),0,0,0.008,0.01,2000)\n"
        <<"  	if point_dist(target_pose, get_forward_kin()) > 0.005:\n"
        <<"  	popup(\"Lower grasping force or max width\",title=\"RG-lag threshold exceeded\", warning=False, error=False, blocking=False)\n"
        <<"  	end\n"
        <<"  	end\n"
        <<"  	nspeed = norm(get_actual_tcp_speed())\n"
        <<"  	while nspeed > 0.001:\n"
        <<"  	servoj(get_inverse_kin(target_pose),0,0,0.008,0.01,2000)\n"
        <<"  	nspeed = norm(get_actual_tcp_speed())\n"
        <<"  	end\n"
        <<"  	end\n"
        <<"  	if depth_compensation==False:\n"
        <<"  	timeout = 0\n"
        <<"  	while get_digital_in(9) == True:\n"
        <<"  	  timeout = timeout+1\n"
        <<"  	  sync()\n"
        <<"  	  if timeout > 20:\n"
        <<"  	    break\n"
        <<"  	  end\n"
        <<"  	end\n"
        <<"  	timeout = 0\n"
        <<"  	while get_digital_in(9) == False:\n"
        <<"  	  timeout = timeout+1\n"
        <<"  	  sync()\n"
        <<"  	  if timeout > 400:\n"
        <<"  	    break\n"
        <<"  	  end\n"
        <<"  	end\n"
        <<"  	end\n"
        <<"  	sync()\n"
        <<"  	sync()\n"
        <<"  	sync()\n"
        <<"  	if set_payload:\n"
        <<"  	if slave:\n"
        <<"  	if get_analog_in(3) < 2:\n"
        <<"  	zslam=0\n"
        <<"  	else:\n"
        <<"  	zslam=payload\n"
        <<"  	end\n"
        <<"  	else:\n"
        <<"  	if get_digital_in(8) == False:\n"
        <<"  	zmasm=0\n"
        <<"  	else:\n"
        <<"  	zmasm=payload\n"
        <<"  	end\n"
        <<"  	end\n"
        <<"  	zload=zmasm+zslam+zsysm\n"
        <<"  	set_payload(zload,[(zsysx*zsysm+zmasx*zmasm+zslax*zslam)/zload,(zsysy*zsysm+zmasy*zmasm+zslay*zslam)/zload,(zsysz*zsysm+zmasz*zmasm+zslaz*zslam)/zload])\n"
        <<"  	end\n"
        <<"  	master_grip_detected=False\n"
        <<"  	master_lost_grip=False\n"
        <<"  	slave_grip_detected=False\n"
        <<"  	slave_lost_grip=False\n"
        <<"  	if True == get_digital_in(8):\n"
        <<"  		master_grip_detected=True\n"
        <<"  	end\n"
        <<"  	if get_analog_in(3)>2:\n"
        <<"  		slave_grip_detected=True\n"
        <<"  	end\n"
        <<"  	grip_detected=False\n"
        <<"  	lost_grip=False\n"
        <<"  	if True == get_digital_in(8):\n"
        <<"  		grip_detected=True\n"
        <<"  	end\n"
        <<"  	zscale = (get_analog_in(2)-0.026)/2.976\n"
        <<"  	zangle = zscale*1.57079633-0.0942477796\n"
        <<"  	zwidth = 8.4+160*sin(zangle)\n"
        <<"  	global measure_width = (floor(zwidth*10))/10-9.2\n"
        <<"  	if slave:\n"
        <<"  	slave_measure_width=measure_width\n"
        <<"  	else:\n"
        <<"  	master_measure_width=measure_width\n"
        <<"  	end\n"
        <<"  	return grip_detected\n"
        <<"  end\n"
        <<"  set_tool_voltage(24)\n"
        <<"  set_tcp(p[-0,-0,0.23435,0,-0,0])\n"
        <<"  # end: URCap Installation Node\n"
        <<"    $ 1 \"Robot Program\"\n"
        <<"    # begin: URCap Program Node\n"
        <<"    #   Source: RG - On Robot, 1.2.1, On Robot ApS\n"
        <<"    #   Type: RG6\n"
        <<"    $ 2 \"RG6(10)\"\n"
        <<"    RG6("<<target_width<<","<<target_force<<","<<payload<<","<<set_payload<<","<<depth_compensation<<","<<slave<<")\n"
        <<"    # end: URCap Program Node   \n"
        <<"  set_tcp(p[0,0,0,0,0,0])\n";
          return oss.str();

    }


    static std::string ResetTCPScript()
    {
        return "set_tcp(p[0,0,0,0,0,0])\n";

    }
          static std::string GetTCPPoseScript(std::string ip, unsigned short port, std::string extraCommand="", std::string functionName="fc")
        {
          std::ostringstream oss;
          oss<<"def "<<functionName<<"():\n"
            <<"socket_open(\""<<ip<<"\","<<port<<")\n"
           <<extraCommand
          <<"w=get_actual_tcp_pose()\n"
         <<"socket_send_line(w)\n"
        <<"end\n";
          return oss.str();
    }

          template<typename T>
          static std::string SetTCPPoseResScript(T x, T y, T z, T rx, T ry, T rz, std::string ip, unsigned short port, T a=0.02, T v=0.1, char f='j', std::string functionName="fc")
        {
          return GetTCPPoseScript(ip, port, SetTCPPoseScript(x,y,z,rx,ry,rz,a,v,f), functionName);
    }

          template<typename T>
          static std::string RG2ResScript(T openmm, std::string ip, unsigned short port, std::string functionName="fc")
        {
          return GetTCPPoseScript(ip, port, RG2Script(openmm), functionName);
    }

          static bool LoadScript(std::string &script, std::string filePath)
        {
          std::ifstream ifs(filePath.c_str());
          if(ifs.is_open())
        {
          script.clear();
          std::string str;
          while (std::getline(ifs, str))
          script+=str+"\n";
          ifs.close();
          return true;
    }
          std::cout<<"LoadScript "<<filePath<<" error\n";
          return false;
    }

          template<typename T>
          static void DecodeTCPResult(std::string result, T &x, T &y, T &z, T &rx, T &ry, T &rz)
        {
          char seg;
          std::istringstream iss(result.substr(2, result.size()-2));
          iss>>x>>seg;
          iss>>y>>seg;
          iss>>z>>seg;
          iss>>rx>>seg;
          iss>>ry>>seg;
          iss>>rz>>seg;
    }



          URScript()
        {

    }

    };
#endif // URSCRIPT_HPP
